/*
	Line Sensor Functions
	Updated: 2/13/08
	
	Created By: Jared Bayne
	E-Mail:		JaredNBayne@gmail.com
	Created On:	February 13th, 2008

	Purpose:	This file will handle all of the line sensor functions.
				Probably most importantly, it will hold the defines for
				each sensor
				
		NOTE: 	Please refer to document "Line_Sensor_Connections.ods" for 
				line sensor definitions.  Also need to update addresses of the line
				sensors (FLS1, FLS2 etc) in the #define section below.


*/

#include "cpu2comms.h"

//Misc Defines
#define white 0
#define black 1

#define	Average_Factor	100	
#define	MUX_Delay	1		//not sure if need this, this is just a test	

//Defines for front line sensor board
/*

	Line Sensor Defines  ***BY JARED BAYNE   JaredNBayne@gmail.com  4/16/09*********
	
	Please refer to document Line_Sensor_Connection.ods for a list of line sensor inputs and outputs
	
	
	Here's the low down:
	
	The CPU1 (main CPU) sends a request to CPU2.  The request has an integer in it that corresponds
	to a line sensor connector position.  The "line sensor connector" I'm referring to is that big 40
	pin connector on the back of the robot.  
	
	For example:
	CPU1 wants to read the sensor plugged in to line sensor connector position 21.
	
	To do this, CPU1 will send a request with the number 30 in it.  So, in that document referenecd above,
	look at the column "Line Sensor Connect".  This is the physical pin on that connector.  The column "CPU1 Request"
	is what the CPU1 sends to request that line sensor connector position.  All that data in the columns in between
	these two shows the routing of the data through the schmidtt triggers and multiplexers.
	
	Here is how the Line Sensor Connector pins are defined:
	
	1	2	3	4	5	6	7	8	9	10	11	12	13	14	15	16	17	18	19	20
	21	22	23	24	25	26	27	28	29	30	31	32	33	34	35	36	37	38	39	40
	
	I know it's not the typical way to number pins, but I didn't really think about it when I made it up.
	Look for the pin that is circled black *on the bottomside of the board by all the soldering) for pin
	number 1.  The numbers above are as if you are looking at the bottom of the board.  Pin 1 is the closest
	to the corner of the perf board.
	
	
	Now, you might be thinking, why don't you just set the "request" number to be the pin number on the connector?
	Well, in onrder to minimize pins needed to connect to CPU2 the following scheme was used for addressing.
	
	Address Lines 1-4: Select which line sensor from MUX to pick
	Address Liens 5-6: Select which MUX to get the line sensor from.
	
	See, if uses less pins than your idea!
	
	But you don't have to worry about this addressing scheme (the 1-4 and 5-6 part).  Just look at the document
	to see what number corresponds to what pin.
	
	You can also look at my documentation on how CPU2 works in that code.
	
	
	
2009 SETUP
As you look at the robot from above, we are going to call the line
sensor on the far left (which is right next to the front driver side
wheel) sensor 1.


We are using connector pins:
3,4,5,22,23,24,25

Connect		Sensor
  3		- 		6
  4		-		5
  5		-		2
 22		-		7
 23		-		4
 24		-		3
 25		-		1
 
 
 4-15-2009 UPDATE
 We are no longer using CPU2.  Line sensors are hard wired into CPU1!*/


//Another row on MOBOARD Connector
#define		FLS1	GET_BIT(PINB).bit3
#define		FLS2	GET_BIT(PINB).bit0
#define		FLS3	GET_BIT(PIND).bit7
#define		FLS4	GET_BIT(PIND).bit5
#define		FLS5	GET_BIT(PIND).bit6
#define		FLS6	GET_BIT(PIND).bit4
#define		FLS7	GET_BIT(PIND).bit1
//#define		FLS8	GET_BIT(PINB).bit0		//not using this one right now
//#define		FLS9	GET_BIT(PINB).bit3		//not using this one right now




#define Num_Of_LS	7	//

//Global Variables...I know, it's a global...deal with it   -Jared
int FLS[Num_Of_LS]; 


//Public Functions
int Check_All_Front(void);
void Read_Front(void);    // fills the array with line sensor data
void Disp_FLS(void);	// sends to LCD screen 
void Serial_Disp_FLS(void);// sends to lap top line sensor info
double Find_Line_Center(void);
int	how_many_see_black(void);
void output_every_sensor(void);
int see_black_line_and_confirm(int delay);
void line_sensor_tester(void);

void line_sensor_tester(void)
{

	while(Scan_Button_Press()==0)
	{
		Disp_FLS();
		_delay_ms(250);
	
	
	}

}


int see_black_line_and_confirm(int delay)
{
	if (how_many_see_black()!=0)
	{
		//one of the line sensors sees black (line or edge actually)
		//need to pause to let robot settle then confirm
		
		stop();
		writeln("T:");
		for (int i=0;i<=6;i++)
		{
			if (FLS[i]==black)
				lcddispchar('b');
			else
				lcddispchar('w');
		}
		_delay_ms(delay);
		if (how_many_see_black()!=0)
			return YES;		//sees black line or edge
		else
			return NO;		//false positive, so we don't actually see black line or edge
	}
	
	return NO;		//no black line or edge



}

void output_every_sensor(void)
{
	
	for (int i=0;i<40;i++)
	{
		CPU2_SetAddress(i);		//Set Address Lines for FLS1
		_delay_ms(MUX_Delay);
		usart_writeln("  ",1);
		usart_disp_int(i,1);
		usart_write(": ",1);
		usart_putchar(CPU2Data+48,1);
	}

}

int	how_many_see_black(void)
{

	int temp=0;
	Read_Front();
	for (int i=0;i<Num_Of_LS;i++)
	{
		if ((FLS[i]==black) && (i!=1))
			temp++;
	
	}
	
	if (temp>0)
	{
		stop();
		writeln(": ");
		for (int i=0;i<=6;i++)
		{
			if (FLS[i]==black)
				lcddispchar('b');
			else
				lcddispchar('w');
		}
	
	}
	
	return temp;
}





void Serial_Disp_FLS(void)  // this function talks to the laptop
{
	Read_Front();
	usart_writeln("FLS: ",1);
	
	
	//Loop through all 10 line sensors 
	for (int i=0;i<=Num_Of_LS;i++) 
	{
	
		//usart_putchar(FLS[i]+48,1);
		
		
		if (FLS[i]==black)
			usart_putchar('b',1);
		else
			usart_putchar('w',1);
			
	}
		usart_write("   ",1);
		

}

void Disp_FLS(void) // this function sends Front line sensor array info to LCD
{
	
	Read_Front();
	lcdclear();
	for (int i=0;i<=6;i++)
	{
		if (FLS[i]==black)
			lcddispchar('b');
		else
			lcddispchar('w');
	}
	lcdline2();
	for (int i=10;i<=Num_Of_LS-1;i++)
	{
		if (FLS[i]==black)
			lcddispchar('b');
		else
			lcddispchar('w');
	}
}


int Check_All_Front(void)
{
/*
	Function: 	Scan_All_Front
	
	Purpose:	Will scan all the front line sensors.
				
	INPUTS: NONE

	OUTPUTS: 	All Line Sensors White? return white
				All Line Sensors Black? return black
				Line Sensors have different values? return 9
*/
	int flag1=0;
	
	Read_Front();   //fills the array with line sensor data
	
	//Are all black?
	for (int i=0;i<=Num_Of_LS-1;i++)
	{
		if (FLS[i]==white)
			flag1=1;	
	}
	
	if (flag1==0)		//All black
		return black;
		
		
	flag1=0;	
	//Are all white?
	for (int i=0;i<=Num_Of_LS-1;i++)
	{
		if (FLS[i]==black)
			flag1=1;	
	}
	
	if (flag1==0)		//All white
		return white;

	return 9;

}

void Read_Front(void)
{

//	Function: 	Read_Front
	
//	Purpose:	Will fill the front line sensor array
				
//	INPUTS: NONE

//	OUTPUTS: 	Fills FLS[] with sensor status

	FLS[0] = FLS1;
	FLS[1] = FLS2;
	FLS[2] = FLS3;
	FLS[3] = FLS4;
	FLS[4] = FLS5;
	FLS[5] = FLS6;
	FLS[6] = FLS7;
	//FLS[7] = FLS8;		//NOT USING THIS SENSOR NOW
	//FLS[8] = FLS9;		//NOT USING THIS SENSOR NOW

}
